#pragma once

#include "../numeric/i_poly.h"
#include "../numeric/i_nullspace.h"
#include "../optimize/i_lm.h"
#include "i_triangulation.h"
#include "i_util.h"

namespace idl
{
  /*Compute the sampson error distance given two image points and the fundamental/essential matrix*/
  template<typename T> inline T i_sampson_distance_fundamental(const T x1[2], const T x2[2], const T f[9])
  {
    T fx1, fx2, fx3, xf1, xf2, den, num;
    fx1 = f[0] * x1[0] + f[1] * x1[1] + f[2];
    fx2 = f[3] * x1[0] + f[4] * x1[1] + f[5];
    fx3 = f[6] * x1[0] + f[7] * x1[1] + f[8];
    xf1 = f[0] * x2[0] + f[3] * x2[1] + f[6];
    xf2 = f[1] * x2[0] + f[4] * x2[1] + f[7];
    num = x2[0] * fx1 + x2[1] * fx2 + fx3;
    den = fx1*fx1 + fx2*fx2 + xf1*xf1 + xf2*xf2;
    return i_div(num*num, den);
  }

  /*Compute the point to epipolar line error (distance in pixel) given two image points and the fundamental/essential matrix*/
  template<typename T> inline T i_epiline_distance_fundamental(const T x1[2], const T x2[2], const T f[9])
  {
    T l[3];
    l[0] = f[0] * x1[0] + f[1] * x1[1] + f[2];
    l[1] = f[3] * x1[0] + f[4] * x1[1] + f[5];
    l[2] = f[6] * x1[0] + f[7] * x1[1] + f[8];
    return i_div(i_abs(i_dot2(l, x2) + l[2]), i_l2_norm2(l));
  }

  /*Compute the symmetric point to epipolar line error (distance in pixel) given two image points and the fundamental/essential matrix*/
  template<typename T> inline T i_symmetric_epiline_distance_fundamental(const T x1[2], const T x2[2], const T f[9])
  {
    T l[3], err1, err2;
    
    l[0] = f[0] * x1[0] + f[1] * x1[1] + f[2];
    l[1] = f[3] * x1[0] + f[4] * x1[1] + f[5];
    l[2] = f[6] * x1[0] + f[7] * x1[1] + f[8];
    err1 = i_div(i_abs(i_dot2(l, x2) + l[2]), i_l2_norm2(l));
    
    l[0] = f[0] * x2[0] + f[3] * x2[1] + f[6];
    l[1] = f[1] * x2[0] + f[4] * x2[1] + f[7];
    l[2] = f[2] * x2[0] + f[5] * x2[1] + f[8];
    err2 = i_div(i_abs(i_dot2(l, x1) + l[2]), i_l2_norm2(l));
    
    return i_average(err1, err2);
  }

  /*Force 3x3 matrix f to a fundamental matrix and apply scaling so that the leading singular value is 1*/
  template<typename T> inline void i_force_fundamental(T f[9], int max_svd_iterations = I_DEFAULT_MAX_SVD_ITERATIONS)
  {
    T Ut[9], w[3], Vt[9];
    i_svd_3x3(f, Ut, w, Vt, true, max_svd_iterations);
    w[1] = i_div(w[1], w[0]);
    w[0] = (T)1.0;
    w[2] = (T)0.0;
    if ( i_determinant_3x3(Ut) < (T)0.0 ) i_neg9(Ut);
    if ( i_determinant_3x3(Vt) < (T)0.0 ) i_neg9(Vt);
    i_svd_compose_3x3(Ut, w, Vt, f);
  }

  /*Force 3x3 matrix f to an essential matrix and apply scaling so that the first two singular values are 1s*/
  template<typename T> inline void i_force_essential(T f[9], int max_svd_iterations = I_DEFAULT_MAX_SVD_ITERATIONS)
  {
    T Ut[9], w[3], Vt[9];
    i_svd_3x3(f, Ut, w, Vt, true, max_svd_iterations);
    w[0] = (T)1.0;
    w[1] = (T)1.0;
    w[2] = (T)0.0;
    if ( i_determinant_3x3(Ut) < (T)0.0 ) i_neg9(Ut);
    if ( i_determinant_3x3(Vt) < (T)0.0 ) i_neg9(Vt);
    i_svd_compose_3x3(Ut, w, Vt, f);
  }

  /*Decompose a rank-2 fundamental matrix f into [t]_xM, where [t]_x is a singular 3x3 skew-symmetric matrix and M is an arbitrary 3x3 Matrix,
    Note that this routine does not check if f is singular and assumes f is perfectly rank-2 or "i_force_fundamental" has been called*/
  template<typename T> inline void i_decompose_fundamental(const T f[9], T M[9], T t[3])
  {
    T ft[9], t_x[9];
    i_transpose_3x3(f, ft);
    i_nullspace_svd_3x3(ft, t);
    i_axiator(t, t_x);
    i_mult_AB_3x3_3x3(t_x, f, M);
  }

  /*Compute the essential matrix given fundamental matrix and camera intrinsic matrix*/
  template<typename T> inline void i_essential_from_fundamental(const T K1[9], const T K2[9], const T f[9], T e[9])
  {
    i_mult_AtBC_3x3_3x3_3x3(K2, f, K1, e);
  }
  template<typename T> inline void i_essential_from_fundamental(const T K[9], const T f[9], T e[9])
  {
    i_mult_AtBC_3x3_3x3_3x3(K, f, K, e);
  }

  /*Compute the fundamental matrix given essential matrix and camera intrinsic matrix*/
  template<typename T> inline void i_fundamental_from_essential(const T K1[9], const T K2[9], const T e[9], T f[9])
  {
    T K1_inv[9], K2_inv[9];
    i_invert_3x3_upper_triangular(K1, K1_inv);
    i_invert_3x3_upper_triangular(K2, K2_inv);
    i_mult_AtBC_3x3_3x3_3x3(K2_inv, e, K1_inv, f);
  }
  template<typename T> inline void i_fundamental_from_essential(const T K[9], const T e[9], T f[9])
  {
    T K_inv[9];
    i_invert_3x3_upper_triangular(K, K_inv);
    i_mult_AtBC_3x3_3x3_3x3(K_inv, e, K_inv, f);
  }

  /*Given a pair of normalized camera matrices P1=[R1|t1] and P2=[R2|t2], this routine computes the essential matrix E.*/
  template<typename T> inline void i_essential_from_normalized_P_matrices(const T P1[12], const T P2[12], T E[9])
  {
    T c1[3], c2[3], e[3], ex[9], R1[9], R2[9], ex_R1t[9];
    i_cop_from_normalized_P_matrix(P1, c1);
    i_cop_from_normalized_P_matrix(P2, c2);
    i_decompose_R_from_normalized_P_matrix(P1, R1);
    i_decompose_R_from_normalized_P_matrix(P2, R2);
    i_sub3(c1, c2, e);
    i_axiator(e, ex);
    i_mult_ABt_3x3_3x3(ex, R1, ex_R1t);
    i_mult_AB_3x3_3x3(R2, ex_R1t, E);
  }

  /*Given a pair of cameras K1[R1|t1] and K2[R2|t2], this routine computes the fundamental matrix F.*/
  template<typename T> inline void i_fundamental_from_K_R_t(const T K1[9], const T R1[9], const T t1[3], 
                                                            const T K2[9], const T R2[9], const T t2[3], T F[9])
  {
    T c1[3], c2[3], e[3], ex[9], ex_R1t[9], E[9], Kinv[9], K2tF[9];
    i_cop_from_R_t(R1, t1, c1);
    i_cop_from_R_t(R2, t2, c2);
    i_sub3(c1, c2, e);
    i_axiator(e, ex);
    i_mult_ABt_3x3_3x3(ex, R1, ex_R1t);
    i_mult_AB_3x3_3x3(R2, ex_R1t, E);
    i_invert_3x3_upper_triangular(K1, Kinv);
    i_mult_AB_3x3_3x3(E, Kinv, K2tF);
    i_invert_3x3_upper_triangular(K2, Kinv);
    i_mult_AtB_3x3_3x3(Kinv, K2tF, F);
  }

  /*Construct the epipolar geometry constraint from a pair of matched points, 
    i.e., given (x,y,1) and (x',y',1) in homogeneous coordinates, construct vector 
    v = [x'x, x'y, x', y'x, y'y, y', x, y, 1], such that vf = 0. input points 
    should be pre-normalized*/
  template<typename T> inline void i_two_point_epipolar_constraint(const T x[3], const T xp[3], T v[9])
  {
    v[0] = x[0] * xp[0]; v[1] = x[1] * xp[0]; v[2] = x[2] * xp[0];
    v[3] = x[0] * xp[1]; v[4] = x[1] * xp[1]; v[5] = x[2] * xp[1];
    v[6] = x[0] * xp[2]; v[7] = x[1] * xp[2]; v[8] = x[2] * xp[2];
  }

  /*Construct the epipolar geometry constraint (fundamental matrix constraint) 
    from five matched pointsin in homogeneous coordinates, construct a 5x9 
    matrix A, such that Af = 0. input points should be pre-normalized*/
  template<typename T> inline void i_five_point_epipolar_constraint(const T x[15], const T xp[15], T A[45])
  {
    i_two_point_epipolar_constraint(x,      xp,      A);
    i_two_point_epipolar_constraint(x + 3,  xp + 3,  A + 9);
    i_two_point_epipolar_constraint(x + 6,  xp + 6,  A + 18);
    i_two_point_epipolar_constraint(x + 9,  xp + 9,  A + 27);
    i_two_point_epipolar_constraint(x + 12, xp + 12, A + 36);
  }

  /*Construct the epipolar geometry constraint (fundamental matrix constraint)
  from seven matched pointsin in homogeneous coordinates, construct a 7x9
  matrix A, such that Af = 0. input points should be pre-normalized*/
  template<typename T> inline void i_seven_point_epipolar_constraint(const T x[21], const T xp[21], T A[63])
  {
    i_two_point_epipolar_constraint(x, xp, A);
    i_two_point_epipolar_constraint(x + 3, xp + 3, A + 9);
    i_two_point_epipolar_constraint(x + 6, xp + 6, A + 18);
    i_two_point_epipolar_constraint(x + 9, xp + 9, A + 27);
    i_two_point_epipolar_constraint(x + 12, xp + 12, A + 36);
    i_two_point_epipolar_constraint(x + 15, xp + 15, A + 45);
    i_two_point_epipolar_constraint(x + 18, xp + 18, A + 54);
  }

  /*Construct the epipolar geometry constraint (fundamental matrix constraint)
  from eight matched pointsin in homogeneous coordinates, construct a 8x9
  matrix A, such that Af = 0. input points should be pre-normalized*/
  template<typename T> inline void i_eight_point_epipolar_constraint(const T x[24], const T xp[24], T A[72])
  {
    i_two_point_epipolar_constraint(x,      xp,      A);
    i_two_point_epipolar_constraint(x + 3,  xp + 3,  A + 9);
    i_two_point_epipolar_constraint(x + 6,  xp + 6,  A + 18);
    i_two_point_epipolar_constraint(x + 9,  xp + 9,  A + 27);
    i_two_point_epipolar_constraint(x + 12, xp + 12, A + 36);
    i_two_point_epipolar_constraint(x + 15, xp + 15, A + 45);
    i_two_point_epipolar_constraint(x + 18, xp + 18, A + 54);
    i_two_point_epipolar_constraint(x + 21, xp + 21, A + 63);
  }

  /*Multiply first degree E1 and E2 in x y z into second degree p in x y z, where
  E1[0]; x
  E1[9]; y
  E1[18];z
  E1[27];1
  and likewise for E2 and
  p[0];x2
  p[1];y2
  p[2];z2
  p[3];xy
  p[4];xz
  p[5];yz
  p[6];x
  p[7];y
  p[8];z
  p[9];1*/
  template<typename T> inline void i_five_point_polymult_o1(const T *E1, const T *E2, T p[10])
  {
    p[0] = E1[0]  * E2[0]; /*x2*/
    p[1] = E1[9]  * E2[9]; /*y2*/
    p[2] = E1[18] * E2[18];/*z2*/
    p[3] = E1[0]  * E2[9]  + E1[9]  * E2[0];/*xy*/
    p[4] = E1[0]  * E2[18] + E1[18] * E2[0];/*xz*/
    p[5] = E1[9]  * E2[18] + E1[18] * E2[9];/*yz*/
    p[6] = E1[0]  * E2[27] + E1[27] * E2[0];/* x*/
    p[7] = E1[9]  * E2[27] + E1[27] * E2[9];/* y*/
    p[8] = E1[18] * E2[27] + E1[27] * E2[18];/*z*/
    p[9] = E1[27] * E2[27];/*1*/
  }

  /*Multiply second degree p in x y z with first degree E in x y z, where
  p[0];x2
  p[1];y2
  p[2];z2
  p[3];xy
  p[4];xz
  p[5];yz
  p[6];x
  p[7];y
  p[8];z
  p[9];1
  and
  E[0]; x
  E[9]; y
  E[18];z
  E[27];1
  and A has the terms x3 y3 x2y xy2 x2z x2 y2z y2 xyz xy xz2 xz x yz2 yz y z3 z2 z 1
  as
  A[0];x3
  A[1];y3
  A[2];x2y
  A[3];xy2
  A[4];x2z
  A[5];x2
  A[6];y2z
  A[7];y2
  A[8];xyz
  A[9];xy
  A[10];x
  A[11];xz
  A[12];xz2
  A[13];y
  A[14];yz
  A[15];yz2
  A[16];1
  A[17];z
  A[18];z2
  A[19];z3*/
  template<typename T> inline void i_five_point_polymult_o2(const T p[10], const T *E, T A[20])
  {
    A[0]  = p[0] * E[0];/*x3*/
    A[1]  = p[1] * E[9];/*y3*/
    A[2]  = p[0] * E[9]  + p[3] * E[0];/*x2y*/
    A[3]  = p[1] * E[0]  + p[3] * E[9];/*xy2*/
    A[4]  = p[0] * E[18] + p[4] * E[0];/*x2z*/
    A[5]  = p[0] * E[27] + p[6] * E[0];/*x2*/
    A[6]  = p[1] * E[18] + p[5] * E[9];/*y2z*/
    A[7]  = p[1] * E[27] + p[7] * E[9];/*y2*/
    A[8]  = p[3] * E[18] + p[4] * E[9] + p[5] * E[0];/*xyz*/
    A[9]  = p[3] * E[27] + p[6] * E[9] + p[7] * E[0];/*xy*/
    A[10] = p[6] * E[27] + p[9] * E[0];/*x*/
    A[11] = p[4] * E[27] + p[6] * E[18] + p[8] * E[0];/*xz*/
    A[12] = p[2] * E[0]  + p[4] * E[18];/*xz2*/
    A[13] = p[7] * E[27] + p[9] * E[9];/*y*/
    A[14] = p[5] * E[27] + p[7] * E[18] + p[8] * E[9];/*yz*/
    A[15] = p[2] * E[9]  + p[5] * E[18];/*yz2*/
    A[16] = p[9] * E[27];/*1*/
    A[17] = p[8] * E[27] + p[9] * E[18];/*z*/
    A[18] = p[2] * E[27] + p[8] * E[18];/*z2*/
    A[19] = p[2] * E[18];/*z3*/
  }

  /*Compute the trace constraint given nullspace*/
  template<typename T> inline void i_five_point_essential_constraint(const T ns[36], T A[200])
  {
    int i3;
    T p[10], q[10], a[20];
    T EEt[9][10]; /*10 coefficients*/
    T *EEt_p, *A_p;
    const T *nsi, *nsj;

    /*determinant equation*/
    i_five_point_polymult_o1(ns + 1, ns + 5, p);
    i_five_point_polymult_o1(ns + 2, ns + 4, q);
    i_sub10(q, p);
    i_five_point_polymult_o2(p, ns + 6, A + 180);

    i_five_point_polymult_o1(ns + 2, ns + 3, p);
    i_five_point_polymult_o1(ns,     ns + 5, q);
    i_sub10(q, p); 
    i_five_point_polymult_o2(p, ns + 7, a);
    i_add20(a, A+180);  

    i_five_point_polymult_o1(ns,     ns + 4, p);
    i_five_point_polymult_o1(ns + 1, ns + 3, q);
    i_sub10(q, p);
    i_five_point_polymult_o2(p, ns + 8, a);
    i_add20(a, A+180); 

    /*EEt*/
    for (int i=0; i<3; i++)
    {
      i3 = i*3;
      for (int j=i; j<3; j++)
      {
        EEt_p = EEt[i3 + j];
        nsi = ns +  i3;
        nsj = ns + j*3;
        i_five_point_polymult_o1(nsi,     nsj,    EEt_p);
        i_five_point_polymult_o1(nsi + 1, nsj + 1,    q);
        i_add10(q, EEt_p);
        i_five_point_polymult_o1(nsi + 2, nsj + 2,    q);
        i_add10(q, EEt_p);
      }
    }

    /*copy the symmetric parts*/
    i_copy10(EEt[1], EEt[3]); 
    i_copy10(EEt[2], EEt[6]); 
    i_copy10(EEt[5], EEt[7]);
    /*sum up diagonal*/
    i_copy10(EEt[0], q);
    i_add10 (EEt[4], q);
    i_add10 (EEt[8], q);
    /*subtract (diagonal sum)/2 from EEt*/
    i_sub_scaled(q, EEt[0], 10, (T)(0.5));
    i_sub_scaled(q, EEt[4], 10, (T)(0.5));
    i_sub_scaled(q, EEt[8], 10, (T)(0.5));

    /*final matrix multiplication (EEt - 0.5*trace(EEt))E*/
    A_p = A;
    for (int i=0; i<3; i++)
    {
      i3 = i*3;
      for (int j = 0; j<3; j++)
      {
        i_five_point_polymult_o2(EEt[i3],   ns+j,   A_p);
        i_five_point_polymult_o2(EEt[i3+1], ns+3+j,  a);
        i_add20(a, A_p); 
        i_five_point_polymult_o2(EEt[i3+2], ns+6+j,  a);
        i_add20(a, A_p);
        A_p += 20;
      }
    }
  }

  /*Cancel out z according to the equation system table in Nister's "Five-Point Relative Pose" paper*/
  template<typename T> inline void i_five_point_cancel_z(const T* A, T B[3][5])
  {
    i_zero5(B[0]); i_copy3(A + 10, B[0]); i_sub3(A + 30, B[0] + 1);
    i_zero5(B[1]); i_copy3(A + 13, B[1]); i_sub3(A + 33, B[1] + 1);
    i_zero5(B[2]); i_copy4(A + 16, B[2]); i_sub4(A + 36, B[2] + 1);
  }

  /*Compute tenth degree polynomial and cofactors p1 p2 p3*/
  template<typename T> inline void i_five_point_poly(const T A[200], T p[11], T p1[8], T p2[8], T p3[7])
  {
    /*3x3 matrix B containing polynomials in z (polynomials up to degree 4 => up to 5 coefficients)*/
    T B[3][3][5];
    i_five_point_cancel_z(A + 80,  B[0]);
    i_five_point_cancel_z(A + 120, B[1]);
    i_five_point_cancel_z(A + 160, B[2]);

    /*construct the tenth degree polynomial using the determinant constraint (determinant of B must vanish)*/
    /*compute the three subdeterminants*/
    i_poly_mult(B[0][1], B[1][2], p1, 3, 4);
    i_poly_mult_sub(B[0][2], B[1][1], p1, 4, 3);
 
    i_poly_mult(B[0][2], B[1][0], p2, 4, 3);
    i_poly_mult_sub(B[0][0], B[1][2], p2, 3, 4);
    
    i_poly_mult(B[0][0], B[1][1], p3, 3, 3);
    i_poly_mult_sub(B[0][1], B[1][0], p3, 3, 3);
    /*final determinant*/
    i_poly_mult(p1,     B[2][0], p, 7, 3);
    i_poly_mult_add(p2, B[2][1], p, 7, 3);
    i_poly_mult_add(p3, B[2][2], p, 6, 4);
  }

  /*Solve for the up to three fundamental matrices in
  the pencil defined by nullspace vectors a and b. It is assumed that b is the 
  last vector of a nullspace, and hence the solution is likely to be closer to 
  b than to a, or, more exactly, it is assumed that the solution is not exactly a*/
  template<typename T>
  inline int i_fundamentals_from_seven_point_nullspace(const T a[9], const T b[9], T fs[27])
  {
    /*The fundamental matrix is in the pencil ax+b. Compute the determinant,
    which is a cubic polynomial in x*/
    T P[3][3][2];
    T D12[3], D13[3], D23[3];
    T c[4], roots[3];
    int i, j=0, nr_solutions = 0;

    i_zero(fs, 27);

    P[0][0][0] = b[0]; P[0][0][1] = a[0];
    P[0][1][0] = b[1]; P[0][1][1] = a[1];
    P[0][2][0] = b[2]; P[0][2][1] = a[2];
    P[1][0][0] = b[3]; P[1][0][1] = a[3];
    P[1][1][0] = b[4]; P[1][1][1] = a[4];
    P[1][2][0] = b[5]; P[1][2][1] = a[5];
    P[2][0][0] = b[6]; P[2][0][1] = a[6];
    P[2][1][0] = b[7]; P[2][1][1] = a[7];
    P[2][2][0] = b[8]; P[2][2][1] = a[8];

    /*Compute the 2x2 sub-determinants*/
    i_poly_mult_1_1(P[0][0], P[1][1], D12);
    i_sub_poly_mult_1_1(P[0][1], P[1][0], D12);

    i_poly_mult_1_1(P[0][0], P[1][2], D13);
    i_sub_poly_mult_1_1(P[0][2], P[1][0], D13);

    i_poly_mult_1_1(P[0][1], P[1][2], D23);
    i_sub_poly_mult_1_1(P[0][2], P[1][1], D23);

    /*Expansion by minor along the third row*/
    i_poly_mult_1_2(P[2][0], D23, c);
    i_sub_poly_mult_1_2(P[2][1], D13, c);
    i_add_poly_mult_1_2(P[2][2], D12, c);

    /*Solve cubic polynomial equation*/
    nr_solutions = i_cubic_solve_closed(c, roots);

    /*Expand the fundamental matrix solutions*/
    for (i = 0; i < nr_solutions; i++, j+=9)
    {
      i_add_scaled9(b, a, fs + j, roots[i]);
    }
    return nr_solutions;
  }

  /*Use five point method to solve for fundamental matrix f (essential matrix actually with known intrinsic matrix K) 
    such that xp'*f*x=0 (x and xp are expressed in normalized coordinates)
    given exactly five point correspondences in homogeneous coordinates
    return number of real solutions, f must be pre-allocated to size 90 (up to 10 solutions)
    If nullspace_method is :
    0, LU is used to find the nullspace, faster but less accurate 
    1, SVD is used to find the nullspace, slower but more accurate
    else, always use SVD*/
  template<typename T> inline int i_five_point(const T x[15], const T xp[15], T f[90], 
                                               int null_space_method = 1,
                                               int nr_sturm_iterations = I_DEFAULT_MAX_POLY_STURMCHAIN_ITERATIONS, 
                                               int nr_bisec_iterations = I_DEFAULT_MAX_POLY_BISECTION_ITERATIONS)
  {
    T Af[45];/*5x9 matrix constructed from the fundamental matrix constraint, given five matched points*/
    T ns[36];/*null space of Af*/
    T A[200];/*20x10 equation system after Gauss-Jordan elimination*/
    int iv[10];/*index vector for Gauss-Jordan elimination*/
    T p[11], r[10]; /*tenth-degree polynomial and roots - up to 10 roots */
    T p1[8], p2[8], p3[7];/*cofactors*/
    T xv, yv, zv, p1v, p2v, p3v;
    int i, j=0, nr_solutions=0;
    T *f_ref = (T*)NULL;
    /*construct the fundamental matrix constraint Af from five point correspondences*/
    i_five_point_epipolar_constraint(x, xp, Af);
    /*compute the nullspace of Af, assuming Af has full rank 5, Af is destroyed after this call*/ 
    switch(null_space_method)
    {
      case 0:  i_nullspace_lu_5x9_destroy(Af, ns); break;
      case 1:  i_nullspace_svd_5x9(Af, ns); break;
      default: i_nullspace_svd_5x9(Af, ns); break;
    }
    /*expand cubic constraint*/
    i_five_point_essential_constraint(ns, A);
    /*perform Gauss-Jordan elimination with row pivoting*/
    i_gauss_jordan_row_pivot(A, 10, 20, iv);
    /*find the tenth-degree polynomial*/
    i_five_point_poly(A, p, p1, p2, p3);
    /*extracr roots using the sturm chain method*/
    nr_solutions = i_poly_solve_sturm10(p, r, nr_sturm_iterations, nr_bisec_iterations);
    /*back-substitute and record f*/
    for (i = 0; i < nr_solutions; i++, j+=9)
    {
      zv = r[i];
      p1v = i_poly_eval7(p1, zv);
      p2v = i_poly_eval7(p2, zv);
      p3v = i_poly_eval6(p3, zv);
      xv  = i_div(p1v, p3v);
      yv  = i_div(p2v, p3v);
      f_ref = f + j;
      i_copy9(ns + 27,       f_ref);
      i_add_scaled9(ns,      f_ref, xv); 
      i_add_scaled9(ns + 9,  f_ref, yv); 
      i_add_scaled9(ns + 18, f_ref, zv);
    }
    return nr_solutions;
  }

  /*Use five point method to solve for fundamental matrix f (essential matrix actually with known intrinsic matrix K) 
    such that xp'*f*x=0 (x and xp are expressed in normalized coordinates)
    given exactly five point correspondences in homogeneous coordinates and the disambiguate with the score from a sixth point.
    Number of solutions (0 or 1) will be returned.
    If nullspace_method is :
    0, LU is used to find the nullspace, faster but less accurate 
    1, SVD is used to find the nullspace, slower but more accurate
    else, always use SVD*/
  template<typename T> inline int i_five_point_w_six(const T x[18], const T xp[18], T f[9],
                                                     int null_space_method = 1,
                                                     int nr_sturm_iterations = I_DEFAULT_MAX_POLY_STURMCHAIN_ITERATIONS,
                                                     int nr_bisec_iterations = I_DEFAULT_MAX_POLY_BISECTION_ITERATIONS)
  {
    T fs[90];
    T distance, best_dist, x1[2], x2[2];
    int nr_solutions, i, best_i;
    nr_solutions = i_five_point(x, xp, fs, null_space_method, nr_sturm_iterations, nr_bisec_iterations);
    if (!nr_solutions)
    {
      i_eye_3x3(f);
      return 0;
    }
    if (nr_solutions == 1)
    {
      i_copy9(fs, f);
      return 1;
    }
    //homogeneous unitize:
    x1[0] = i_div(x[15],  x[17]);
    x1[1] = i_div(x[16],  x[17]);

    x2[0] = i_div(xp[15], xp[17]);
    x2[1] = i_div(xp[16], xp[17]);

    best_dist = i_sampson_distance_fundamental(x1, x2, fs);
    best_i = 0;
    for (i = 1; i<nr_solutions; i++)
    {
      distance = i_sampson_distance_fundamental(x1, x2, fs + 9*i);
      if (distance < best_dist)
      {
        best_dist = distance;
        best_i = i;
      }
    }
    i_copy9(fs + best_i*9, f);
    return 1;
  }

  /*Use seven point method to solve for fundamental matrices f such that xp'*f*x=0
    given exactly seven point correspondences in homogeneous coordinates
    If nullspace_method is :
    0, LU is used to find the nullspace, faster but less accurate
    1, SVD is used to find the nullspace, slower but more accurate
    else, always use SVD*/
  template<typename T> inline int i_seven_point(const T x[21], const T xp[21], T f[27], int null_space_method = 1, int force_fundamental = 1)
  {
    T A[63], ns[18];
    int i, j = 0, nr_solutions = 0;
    i_seven_point_epipolar_constraint(x, xp, A);
    switch (null_space_method)
    {
    case 0:  i_nullspace_lu_7x9_destroy(A, ns); break;
    case 1:  i_nullspace_svd_7x9(A, ns); break;
    default: i_nullspace_svd_7x9(A, ns); break;
    }
    nr_solutions = i_fundamentals_from_seven_point_nullspace(ns, ns + 9, f);
    if (force_fundamental)
    {
      for (i = 0; i < nr_solutions; ++i, j+=9)
      {
        i_force_fundamental(f+j);
      }
    }
    return nr_solutions;
  }

  /*Use seven point method to solve for fundamental matrix f such that xp'*f*x=0 (x, xp are both image coordinates without applying normalization)
  given exactly seven point correspondences in homogeneous coordinates
  If nullspace_method is :
  0, LU is used to find the nullspace, faster but less accurate
  1, SVD is used to find the nullspace, slower but more accurate
  else, always use SVD*/
  template<typename T> inline int i_seven_point_normalized(const T x[21], const T xp[21], T f[27], int null_space_method = 1, int force_fundamental = 1)
  {
    T A[63], ns[18];
    T nx[21], nxp[21];
    T Hx[9], Hxp[9], nf[27];

    //normalize input image coordinates to the normalized space;
    i_point_preconditioning(x,  nx,  Hx,  7);
    i_point_preconditioning(xp, nxp, Hxp, 7);

    int i, j = 0, nr_solutions = 0;
    i_seven_point_epipolar_constraint(nx, nxp, A);
    switch (null_space_method)
    {
    case 0:  i_nullspace_lu_7x9_destroy(A, ns); break;
    case 1:  i_nullspace_svd_7x9(A, ns); break;
    default: i_nullspace_svd_7x9(A, ns); break;
    }
    nr_solutions = i_fundamentals_from_seven_point_nullspace(ns, ns + 9, nf);
    
    //constraint enforcement
    if (force_fundamental)
    {
      for (i = 0, j = 0; i < nr_solutions; ++i, j += 9)
      {
        i_force_fundamental(nf + j);
      }
    }
    //apply the normalizing transformation to cancel out point normalization
    for (i = 0, j = 0; i < nr_solutions; ++i, j += 9)
    {
      i_mult_AtBC_3x3_3x3_3x3(Hxp, nf+j, Hx, f+j);
    }
    return nr_solutions;
  }

  /*Use seven point method to solve for fundamental matrix f such that xp'*f*x=0
    given exactly seven point correspondences in homogeneous coordinates and the disambiguate with the score from a eighth point.
    If nullspace_method is :
    0, LU is used to find the nullspace, faster but less accurate
    1, SVD is used to find the nullspace, slower but more accurate
    else, always use SVD*/
  template<typename T> inline int i_seven_point_w_eight(const T x[24], const T xp[24], T f[9], int null_space_method = 1, int force_fundamental = 1)
  {
    T fs[27];
    T distance, best_dist, x1[2], x2[2];
    int nr_solutions, i, best_i;
    nr_solutions = i_seven_point(x, xp, fs, null_space_method, force_fundamental);
    if (!nr_solutions)
    {
      i_eye_3x3(f);
      return 0;
    }
    if (nr_solutions == 1)
    {
      i_copy9(fs, f);
      return 1;
    }
    //homogeneous unitize:
    x1[0] = i_div(x[21], x[23]);
    x1[1] = i_div(x[22], x[23]);
    
    x2[0] = i_div(xp[21], xp[23]);
    x2[1] = i_div(xp[22], xp[23]);

    best_dist = i_sampson_distance_fundamental(x1, x2, fs);
    best_i = 0;
    for (i = 1; i<nr_solutions; i++)
    {
      distance = i_sampson_distance_fundamental(x1, x2, fs + 9 * i);
      if (distance < best_dist)
      {
        best_dist = distance;
        best_i = i;
      }
    }
    i_copy9(fs + best_i * 9, f);
    return  1;
  }

  /*Use seven point method to solve for fundamental matrix f such that xp'*f*x=0 (x, xp are both image coordinates without applying normalization)
  given exactly seven point correspondences in homogeneous coordinates and the disambiguate with the score from a eighth point.
  If nullspace_method is :
  0, LU is used to find the nullspace, faster but less accurate
  1, SVD is used to find the nullspace, slower but more accurate
  else, always use SVD*/
  template<typename T> inline int i_seven_point_w_eight_normalized(const T x[24], const T xp[24], T f[9], int null_space_method = 1, int force_fundamental = 1)
  {
    T fs[27];
    T distance, best_dist, x1[2], x2[2];
    int nr_solutions, i, best_i;
    nr_solutions = i_seven_point_normalized(x, xp, fs, null_space_method, force_fundamental);
    if (!nr_solutions)
    {
      i_eye_3x3(f);
      return 0;
    }
    if (nr_solutions == 1)
    {
      i_copy9(fs, f);
      return 1;
    }

    //homogeneous unitize:
    x1[0] = i_div(x[21], x[23]);
    x1[1] = i_div(x[22], x[23]);
    x2[0] = i_div(xp[21], xp[23]);
    x2[1] = i_div(xp[22], xp[23]);

    best_dist = i_sampson_distance_fundamental(x1, x2, fs);
    best_i = 0;
    for (i = 1; i<nr_solutions; i++)
    {
      distance = i_sampson_distance_fundamental(x1, x2, fs + 9 * i);
      if (distance < best_dist)
      {
        best_dist = distance;
        best_i = i;
      }
    }
    i_copy9(fs + best_i * 9, f);
    return 1;
  }

  /*Use eight point method to solve for fundamental matrix f such that xp'*f*x=0
    given exactly eight point correspondences in homogeneous coordinates
    If nullspace_method is :
    0, LU is used to find the nullspace, faster but less accurate
    1, SVD is used to find the nullspace, slower but more accurate
    else, always use SVD*/
  template<typename T> inline void i_eight_point(const T x[24], const T xp[24], T f[9], int null_space_method = 1, int force_fundamental = 1)
  {
    T A[72];
    i_eight_point_epipolar_constraint(x, xp, A);
    switch (null_space_method)
    {
    case 0:  i_nullspace_lu_8x9_destroy(A, f); break;
    case 1:  i_nullspace_svd_8x9(A, f); break;
    default: i_nullspace_svd_8x9(A, f); break;
    }
    if (force_fundamental)
    {
      i_force_fundamental(f);
    }
  }

  /*Use eight point method to solve for fundamental matrix f such that xp'*f*x=0 (x, xp are both image coordinates without applying normalization)
  given exactly eight point correspondences in homogeneous coordinates
  If nullspace_method is :
  0, LU is used to find the nullspace, faster but less accurate
  1, SVD is used to find the nullspace, slower but more accurate
  else, always use SVD*/
  template<typename T> inline void i_eight_point_normalized(const T x[24], const T xp[24], T f[9], int null_space_method = 1, int force_fundamental = 1)
  {
    T A[72];
    T nx[24], nxp[24];
    T Hx[9], Hxp[9], nf[9];
    //normalize input image coordinates to the normalized space;
    i_point_preconditioning(x,   nx,  Hx, 8);
    i_point_preconditioning(xp, nxp, Hxp, 8);
    //solve the eight-point relative pose problem
    i_eight_point_epipolar_constraint(nx, nxp, A);
    switch (null_space_method)
    {
    case 0:  i_nullspace_lu_8x9_destroy(A, nf); break;
    case 1:  i_nullspace_svd_8x9(A, nf); break;
    default: i_nullspace_svd_8x9(A, nf); break;
    }
    //constraint enforcement
    if (force_fundamental)
    {
      i_force_fundamental(nf);
    }
    //apply the normalizing transformation to cancel out point normalization
    i_mult_AtBC_3x3_3x3_3x3(Hxp, nf, Hx, f);
  }

  /*This routine is used in testing the four-fold ambiguity of essential matrix, 
  A reconstructed point X will be in front of both cameras in one of these four solutions only. Thus, testing
  with a single point to determine if it is in front of both cameras is sufficient to decide
  between the four different solutions for the camera matrix P,
  Note that the two image points x and xp are in normalized space (K^-1 multiplied), and the first camera is assumed to be canonical [I|0]*/
  template<typename T> inline bool i_relative_pose_cheirality_test(const T R[9], const T t[3], const T x[3], const T xp[3])
  {
    T X[4], P2[12], cop[3], prv[3];
    i_compose_normalized_P_matrix_from_R_t(R,  t,  P2);
    i_triangulate_dlt_canonical(x, xp, P2, X);
    i_cop_from_R_t(R, t, cop);
    i_principal_axis_vector_from_R(R, prv);
    return ( i_point_in_front_of_camera(X, cop, prv) && (X[2] > (T)0.0) );
  }

  /*Given the essential matrix e, compute the relative camera pose between two cameras, assuming the first camera is [I|0].
    There are four possible choices for the second camera pose. The routine uses a single point to decide between the four different 
    solutions. The routine returns the number of valid solution. If the returned value > 1, it means the pose is degenerate, 
    i.e., more than one solutions can explain the camera-point configuration
    Note that this routine assumes that e is a perfect essential matrix, i.e., the first two singular values are 1s or i_force_essential has been called
    Also note that the two image points x and xp are in normalized space (K^-1 multiplied)*/
  template<typename T> inline int i_relative_pose_from_essential(const T e[9], const T x[3], const T xp[3], T R[9], T t[3])
  {
    T ecopy[9], Ut[9], w[3], Vt[9], WVt[9], WtVt[9], UWVt[9], UWtVt[9], u3[3], nu3[3];
    int nr_valid_pose = 0;
    i_copy9(e, ecopy);
    i_svd_3x3(ecopy, Ut, w, Vt);

    if (i_determinant_3x3(Ut) < (T)0.0) i_neg9(Ut);
    if (i_determinant_3x3(Vt) < (T)0.0) i_neg9(Vt);

    i_copy3(Ut + 6, u3);//u3 is the last row of Ut;
    i_neg3(u3, nu3);

    WVt[0] = -Vt[3]; WVt[1] = -Vt[4]; WVt[2] = -Vt[5];
    WVt[3] =  Vt[0]; WVt[4] =  Vt[1]; WVt[5] =  Vt[2];
    WVt[6] =  Vt[6]; WVt[7] =  Vt[7]; WVt[8] =  Vt[8];

    WtVt[0] =  Vt[3]; WtVt[1] =  Vt[4]; WtVt[2] =  Vt[5];
    WtVt[3] = -Vt[0]; WtVt[4] = -Vt[1]; WtVt[5] = -Vt[2];
    WtVt[6] =  Vt[6]; WtVt[7] =  Vt[7]; WtVt[8] =  Vt[8];

    i_mult_AtB_3x3_3x3(Ut, WVt,  UWVt);
    i_mult_AtB_3x3_3x3(Ut, WtVt, UWtVt);

    //hypothesis 1st:
    if (i_relative_pose_cheirality_test(UWVt, u3, x, xp))
    {
      i_copy9(UWVt, R);
      i_copy3(u3, t);
      ++nr_valid_pose;
    }
    //hypothesis 2nd:
    if (i_relative_pose_cheirality_test(UWVt, nu3, x, xp))
    {
      i_copy9(UWVt, R);
      i_copy3(nu3, t);
      ++nr_valid_pose;
    }
    //hypothesis 3rd:
    if (i_relative_pose_cheirality_test(UWtVt, u3, x, xp))
    {
      i_copy9(UWtVt, R);
      i_copy3(u3, t);
      ++nr_valid_pose;
    }
    //hypothesis 4th:
    if (i_relative_pose_cheirality_test(UWtVt, nu3, x, xp))
    {
      i_copy9(UWtVt, R);
      i_copy3(nu3, t);
      ++nr_valid_pose;
    }
    return nr_valid_pose; 
  }

  /*RANSAC solvers:*/
  template<typename T>
  inline void i_five_point_ransac_hypogenfunc(const T x[18], const T xp[18], T f[9])
  {
    i_five_point_w_six(x, xp, f);
  }

  template<typename T>
  inline void i_seven_point_ransac_hypogenfunc(const T x[24], const T xp[24], T f[9])
  {
    i_seven_point_w_eight(x, xp, f, 1, 1);
  }

  template<typename T>
  inline void i_normalized_seven_point_ransac_hypogenfunc(const T x[24], const T xp[24], T f[9])
  {
    i_seven_point_w_eight_normalized(x, xp, f, 1, 1);
  }

  template<typename T>
  inline void i_eight_point_ransac_hypogenfunc(const T x[24], const T xp[24], T f[9])
  {
    i_eight_point(x, xp, f, 1, 1);
  }

  template<typename T>
  inline void i_normalized_eight_point_ransac_hypogenfunc(const T x[24], const T xp[24], T f[9])
  {
    i_eight_point_normalized(x, xp, f, 1, 1);
  }

  /*Note this routine uses the sampson error*/
  template<typename T>
  inline void i_relative_pose_costfunc(const T f[9], const T* x, const T* xp, int n, int &nr_liner, int *inliers, T &cost, T error_tol)
  {
    int i;
    T sampson_err = (T)0.0;
    nr_liner = 0;
    cost = (T)0.0;
    const T *refx = x;
    const T *refxp = xp;
    for (i = 0; i < n; i++)
    {
      sampson_err = i_sampson_distance_fundamental(refx, refxp, f);
      if (sampson_err < error_tol)
      {
        inliers[nr_liner++] = i;
        cost += sampson_err;
      }
      refx += 3;
      refxp += 3;
    }
  }

#if 1
  /*This routine optimize the 3x3 fundamental matrix with a set of 2D<->2D correspondeces using the Levenberg-Marquardt algorithm.
  The input image points {xs, xps} should be in inhomogeneous coordinates.*/
  template<typename T>
  class FundamentalMatrixOptimizer
  {
  public:
    typedef T data_type;
    static const int _m = 12;
    static const int _d = 1;
    FundamentalMatrixOptimizer();
    ~FundamentalMatrixOptimizer();
    void computeCost(const T *c, T *fvec) const;
    void computeJacobian(const T *c, T *J) const;
    int getNumUnknowns() const { return _m; }
    int getNumberResiduals() const { return _d*_n; };
    bool optimize(const T f[9], const T *xs, const T *xps, int n);
    void getFundamental(T f[9]) const;
    void setVerbose(bool verbose) { _verbose = verbose; }
    bool isFailed() const { return !_succeeded; }
  protected:
    T _c[_m]; /*12 parameters to model the singular 3x3 matrix F*/
    int _n;
    const T *_xs;
    const T *_xps;
    bool _succeeded;
    bool _verbose;
  };

  template<typename T>
  FundamentalMatrixOptimizer<T>::FundamentalMatrixOptimizer() : _n(0), _xs(NULL), _xps(NULL), _succeeded(false), _verbose(false)
  {
    i_zero(_c, _m);
  }
  
  template<typename T>
  FundamentalMatrixOptimizer<T>::~FundamentalMatrixOptimizer(){}

  template<typename T>
  void FundamentalMatrixOptimizer<T>::getFundamental(T f[9]) const
  {
    T t_x[9];
    i_axiator(_c+9, t_x);
    i_mult_AB_3x3_3x3(t_x, _c, f);
    i_scale9(f, i_rec(i_l2_norm9(f)));/*perform normalization so ||f|| = 1*/
  }

  /*Note that this routine does not check if f is singular and assumes f is perfectly rank-2 or "i_force_fundamental" has been called*/
  template<typename T>
  bool FundamentalMatrixOptimizer<T>::optimize(const T f[9]/*initial fundamental matrix*/, const T* xs, const T* xps, const int n/*# of 2D<->2D correspondeces*/)
  {
    _succeeded = false;
    if (n==0 || xs == NULL || xps == NULL)
    {
      return false;
    }
    /*alloc error vector fvec*/
    T* fvec = i_alloc<T>(n*_d);
    if (fvec == NULL)
    {
      return false;
    }
    _n = n;
    _xs = xs;
    _xps = xps;
    /*initialize fundamental matrix, perform normalization so ||f|| = 1 - numerically stable*/
    T nf[9];
    i_scale9(f, nf, i_rec(i_l2_norm9(f)));
    i_decompose_fundamental(nf, _c, _c + 9);
    LevenbergMarquardt solver;
    solver.setVerbose(_verbose);
    _succeeded = solver.optimize(*this, _c, fvec);
    i_free(fvec);
    return _succeeded;
  }

  template<typename T>
  void FundamentalMatrixOptimizer<T>::computeCost(const T *c, T *fvec) const
  {
    T t_x[9], f[9], fx1, fx2, fx3, xf1, xf2, den, num;
    i_axiator(c + 9, t_x);
    i_mult_AB_3x3_3x3(t_x, c, f);

    const T *cptr_x  = _xs;
    const T *cptr_xp = _xps;
    T *ptr_f = fvec;

    for (int i = 0; i < _n; ++i)
    {
      fx1 = f[0] * cptr_x[0]  + f[1] * cptr_x[1]  + f[2];
      fx2 = f[3] * cptr_x[0]  + f[4] * cptr_x[1]  + f[5];
      fx3 = f[6] * cptr_x[0]  + f[7] * cptr_x[1]  + f[8];
      xf1 = f[0] * cptr_xp[0] + f[3] * cptr_xp[1] + f[6];
      xf2 = f[1] * cptr_xp[0] + f[4] * cptr_xp[1] + f[7];
      num = cptr_xp[0] * fx1 + cptr_xp[1] * fx2 + fx3;
      den = fx1*fx1 + fx2*fx2 + xf1*xf1 + xf2*xf2; //den is guaranteed positive
      *ptr_f = i_div(num, i_sqrt(den));
      cptr_x  += 2;
      cptr_xp += 2;
      ptr_f   += _d;
    }
  }

  template<typename T>
  void FundamentalMatrixOptimizer<T>::computeJacobian(const T *c, T *J) const
  {
    /*J is _n x 12*/
    int i, j_offset = _m * _d;
    T fx1, fx2, fx3, xf1, xf2, u, v, rec_v, rec_vv;
    T t_x[9], f[9], t[3], d_u[_m], d_v[_m];
    T x0, x1, xp0, xp1;
    T xp1t2mt1, t0mxp0t2, xp0t1mxp1t0, m0x0pm1x1pm2, m3x0pm4x1pm5, m6x0pm7x1pm8, m3mm6xp1, m4mm7xp1, m6xp0mm0, m7xp0mm1, m0xp1mm3xp0, m1xp1mm4xp0;
    T fx1t2, fx2t2, fx1t1mfx2t0;

    const T *cptr_x1  = _xs;
    const T *cptr_x2 = _xps;
    T* ptr_J = J;

    i_copy3(c + 9, t);
    i_axiator(t, t_x);
    i_mult_AB_3x3_3x3(t_x, c, f);
  
    for (i = 0; i < _n; ++i, cptr_x1 += 2, cptr_x2 += 2, ptr_J += j_offset)
    {
      /*precompute variables*/
      x0  = cptr_x1[0];
      x1  = cptr_x1[1];
      xp0 = cptr_x2[0];
      xp1 = cptr_x2[1];
     
      fx1 = f[0] * x0 + f[1]  * x1 + f[2];
      fx2 = f[3] * x0 + f[4]  * x1 + f[5];
      fx3 = f[6] * x0 + f[7]  * x1 + f[8];
      xf1 = f[0] * xp0 + f[3] * xp1 + f[6];
      xf2 = f[1] * xp0 + f[4] * xp1 + f[7];

      u = xp0 * fx1 + xp1 * fx2 + fx3;
      v = fx1*fx1 + fx2*fx2 + xf1*xf1 + xf2*xf2; //v^2
      rec_vv = i_rec(v); //1/(v^2)
      v = i_sqrt(v); //v
      rec_v = i_rec(v);

      /*compute derivatives wrt m0,..., m8, t0, t1, and t2*/
      xp1t2mt1 = (xp1 * t[2]) - t[1];
      t0mxp0t2 = t[0] - (xp0 * t[2]);
      xp0t1mxp1t0 = (xp0 * t[1]) - (xp1 * t[0]);

      m0x0pm1x1pm2 = c[0] * x0 + c[1] * x1 + c[2];
      m3x0pm4x1pm5 = c[3] * x0 + c[4] * x1 + c[5];
      m6x0pm7x1pm8 = c[6] * x0 + c[7] * x1 + c[8];
      m3mm6xp1 = c[3] - c[6] * xp1;
      m4mm7xp1 = c[4] - c[7] * xp1;
      m6xp0mm0 = c[6] * xp0 - c[0];
      m7xp0mm1 = c[7] * xp0 - c[1];
      m0xp1mm3xp0 = c[0] * xp1 - c[3] * xp0;
      m1xp1mm4xp0 = c[1] * xp1 - c[4] * xp0;
      fx1t2 = fx1*t[2];
      fx2t2 = fx2*t[2];
      fx1t1mfx2t0 = fx1*t[1] - fx2*t[0];

      d_u[0] = x0 * xp1t2mt1;     //du/dm0
      d_u[1] = x1 * xp1t2mt1;     //du/dm1
      d_u[2] =      xp1t2mt1;     //du/dm2
      d_u[3] = x0 * t0mxp0t2;     //du/dm3
      d_u[4] = x1 * t0mxp0t2;     //du/dm4
      d_u[5] =      t0mxp0t2;     //du/dm5
      d_u[6] = x0 * xp0t1mxp1t0;     //du/dm6
      d_u[7] = x1 * xp0t1mxp1t0;     //du/dm7
      d_u[8] =      xp0t1mxp1t0;     //du/dm8
      d_u[9] =  m3x0pm4x1pm5 - xp1*m6x0pm7x1pm8;     //du/dt0
      d_u[10] = xp0*m6x0pm7x1pm8 - m0x0pm1x1pm2;     //du/dt1
      d_u[11] = xp1*m0x0pm1x1pm2 - xp0*m3x0pm4x1pm5; //du/dt2

      d_v[0] = rec_v * (fx2t2 * x0 + xf1 * xp1t2mt1);   //dv/dm0 - checked
      d_v[1] = rec_v * (fx2t2 * x1 + xf2 * xp1t2mt1);   //dv/dm1 
      d_v[2] = rec_v * (fx2t2);   //dv/dm2
      d_v[3] = rec_v * (-fx1t2 * x0 + xf1 * t0mxp0t2);   //dv/dm3
      d_v[4] = rec_v * (-fx1t2 * x1 + xf2 * t0mxp0t2);   //dv/dm4
      d_v[5] = rec_v * (-fx1t2);   //dv/dm5
      d_v[6] = rec_v * (fx1t1mfx2t0 * x0 + xf1 * xp0t1mxp1t0);  //dv/dm6
      d_v[7] = rec_v * (fx1t1mfx2t0 * x1 + xf2 * xp0t1mxp1t0); //dv/dm7
      d_v[8] = rec_v * (fx1t1mfx2t0);   //dv/dm8
      d_v[9] = rec_v * (-fx2*m6x0pm7x1pm8 + xf1*m3mm6xp1 + xf2*m4mm7xp1);                              //dv/dt0 -checked
      d_v[10] = rec_v * (fx1*m6x0pm7x1pm8 + xf1*m6xp0mm0 + xf2*m7xp0mm1);                              //dv/dt1 -checked
      d_v[11] = rec_v * (-fx1*m3x0pm4x1pm5 + fx2*m0x0pm1x1pm2 + xf1*m0xp1mm3xp0 + xf2*m1xp1mm4xp0);    //dv/dt2 -checked 

      //////////////////////////////////////////////////////////////////////////
      /*df/dc*/
      ptr_J[0] = (d_u[0] * v - u * d_v[0])*rec_vv;
      ptr_J[1] = (d_u[1] * v - u * d_v[1])*rec_vv;
      ptr_J[2] = (d_u[2] * v - u * d_v[2])*rec_vv;
      ptr_J[3] = (d_u[3] * v - u * d_v[3])*rec_vv;
      ptr_J[4] = (d_u[4] * v - u * d_v[4])*rec_vv;
      ptr_J[5] = (d_u[5] * v - u * d_v[5])*rec_vv;
      ptr_J[6] = (d_u[6] * v - u * d_v[6])*rec_vv;
      ptr_J[7] = (d_u[7] * v - u * d_v[7])*rec_vv;
      ptr_J[8] = (d_u[8] * v - u * d_v[8])*rec_vv;
      ptr_J[9] = (d_u[9] * v - u * d_v[9])*rec_vv;
      ptr_J[10] = (d_u[10] * v - u * d_v[10])*rec_vv;
      ptr_J[11] = (d_u[11] * v - u * d_v[11])*rec_vv;
    }
  }
  //////////////////////////////////////////////////////////////////////////////////////////////////

#endif

#if 0
  /*This routine optimize the 3x3 essential matrix with a set of 2D<->2D correspondeces using the Levenberg-Marquardt algorithm.
  The first camera is assumed to be canonical P = [I|0], and second camera is [R|t], R0, t0 can be solved using the inital estimation of e.
  The input image points {xs, xps} should be in inhomogeneous coordinates.*/
  template<typename T>
  class EssentialMatrixOptimizer
  {
  public:
    typedef T data_type;
    static const int _m = 6;
    static const int _d = 1;
    EssentialMatrixOptimizer();
    ~EssentialMatrixOptimizer();
    void computeCost(const T *c, T *fvec) const;
    void computeJacobian(const T *c, T *J) const;
    int getNumUnknowns() const { return _m; }
    int getNumberResiduals() const { return _d*_n; };
    bool optimize(const T f[9], const T *xs, const T *ps, int n, bool verbose);
    void getEssential(T f[9]) const;
    bool isFailed() const { return !_succeeded; }
  protected:
    T _c[_m]; /*6 parameters to model the camera pose [R|t]*/
    int _n;
    const T *_xs;
    const T *_xps;
    bool _succeeded;
    bool _verbose;
  };
#endif

}  /* namespace idl */